#include "_includes.h"

bool AUTO_TEST = 0;

///////////////
bool RUN_SKILLS = false;
long AUTO_TIME;
#define AUTO_NORMAL_TIME 15000 * 0
#define USE_AUTO_TIMEOUT true

void auto_thread_routine() {
  // 自动准备
  t_auto.reset();  // 自动程序运行计时器清零
  ActionUnit::all_units_reset_enc();
  gyro_reset();
  ////////////////////////////////////////////////////////
  if (AUTO_TEST) {
    // 空字符防止警告:format string is not a string literal
    auto_print_60(0, AutoPrint_1, "TEST%s", "");
    auto_test();
    return;
  }

  ActionUnit::set_all_units_motor_stop_status_on_working();  // 全身工作
  if (RUN_SKILLS) {
    auto_skills();
  } else {  // tournament auto
    if (auto_info.isSide(Side::left)) {
      if (auto_info.route_number == 1) auto_left_1();
      if (auto_info.route_number == 2) auto_left_2();
      if (auto_info.route_number == 3) auto_left_3();
      if (auto_info.route_number == 4) auto_left_4();
      if (auto_info.route_number == 5) auto_left_5();
      if (auto_info.route_number == 6) auto_left_6();
    }

    if (auto_info.isSide(Side::right)) {
      if (auto_info.route_number == 1) auto_right_1();
      if (auto_info.route_number == 2) auto_right_2();
      if (auto_info.route_number == 3) auto_right_3();
      if (auto_info.route_number == 4) auto_right_4();
      if (auto_info.route_number == 5) auto_right_5();
      if (auto_info.route_number == 6) auto_right_6();
    }
  }
}

bool autonomous_running = false;
vex::thread thread_autonomous;

void _autonomous(void) {
  if (RUN_SKILLS)
    AUTO_TIME = 60000;
  else
    AUTO_TIME = AUTO_NORMAL_TIME;
  if (AUTO_TIME == 0) AUTO_TIME = 1000000000;

  autonomous_running = true;
  thread_autonomous = thread(auto_thread_routine);  // start auto thread

  wait(150);
  screen_clear();

  ctrl.Screen.clearLine(2);
  ctrl.Screen.clearLine(3);

#if (USE_AUTO_TIMEOUT)
  while (t_auto.time() < AUTO_TIME && thread_autonomous.joinable()) {
    wait(50);
    ctrl.Screen.setCursor(3, 1);
    ctrl.Screen.print("auto time: %.1f  ", t_auto.time() / 1000.0);
  }
  thread_autonomous.interrupt();  // stop auto thread
  ActionUnit::all_units_stop();
#else
  while (thread_autonomous.joinable()) {
    wait(100);
    ctrl.Screen.setCursor(3, 1);
    ctrl.Screen.print("auto time: %.1f  ", t_auto.time() / 1000.0);
  }
#endif

  ctrl.Screen.setCursor(3, 1);
  ctrl.Screen.print("Auto Time: %.2fs   ", t_auto.time() / 1000.0);
  cout << "#### auto time:" << t_auto.time() << endl << endl << endl;

  autonomous_running = false;
}
